﻿#ifndef AGVTHREADCONTROLLER_H
#define AGVTHREADCONTROLLER_H

#include <QDebug>
#include <QMessageBox>
#include <QObject>
#include <QThread>
#include <array>
#include <cstdint>

#include <QtConcurrent/QtConcurrent>
#include <memory>
#include <mutex>
#include <qfuture.h>

#include "spdlog/logger.h"
#include "spdlog/spdlog.h"

#include "struct.h"

#include "agv_system.h"

struct secondlocatecalc_result_t {
  int32_t axis1_inc;
  int32_t axis2_inc;

  ErrCode err_code;
};

struct initialposition_result_t {
  double result;

  ErrCode err_code;
};

class AGVThreadWorker : public QObject {
  Q_OBJECT
  public:
  explicit AGVThreadWorker(QObject *parent = nullptr);
  ~AGVThreadWorker();

  public slots:
  void slot_do_init(void);
  // void slot_AgvStartMove(double dist, double speed, int type);
  void slot_AgvStartLineMove(double inc_m, double speed_m_s); // 单位 米
  void slot_AgvStartRotateMove(double inc_rad, double speed_rad_s); // 单位 弧度
  void slot_AgvStopMove();
  void slot_AgvInitStart(double x_m, double y_m, double angle, int timeout_ms);
  void slot_AgvInitStop();
  void slot_AgvBackToZero(BackToZeroParams param);
  void slot_AgvStartBackChange(double x_mm, double y_mm);
  void slot_AgvMoveToTarget(double _x_pos, double _y_pos);
  void slot_AgvStartMoveToTargetThread();
  void slot_StopAgvSystem();
  void slot_SecondLocateCalc(SecondLocateParams param);
  void slot_CleanUpAgvTcp();
  void slot_CalculateMapAngle(CalculateMapAngleParams params);

  signals:
  void _sig_CommomMessage(int type, QString msg);

  void _sig_SecondLocateMovePlc(secondlocatecalc_result_t result);
  void _sig_InitialPositionResult(initialposition_result_t result);
  void _sig_CalculateMapAngleResult(CalculateMapAngleResponse result);

  private:
  AGVSystem *agv_system_{nullptr};
  std::shared_ptr<spdlog::logger> logger_{nullptr};

  QFuture<void> movetotarget_thread_future_;
  QFutureWatcher<void> movetotarget_thread_future_watcher_;
  bool is_movetotarget_thread_running_{false};
  mutable std::mutex is_movetotarget_thread_running_mutex_;

  QFuture<initialposition_result_t> initialposition_future_;
  QFutureWatcher<initialposition_result_t> initialposition_future_watcher_;
  bool is_initialposition_running_{false};
  mutable std::mutex is_initialposition_running_mutex_;

  QFuture<secondlocatecalc_result_t> secondlocatecalc_future_;
  QFutureWatcher<secondlocatecalc_result_t> secondlocatecalc_future_watcher_;
  bool is_secondlocatecalc_running_{false};
  mutable std::mutex is_secondlocatecalc_running_mutex_;

  QFuture<CalculateMapAngleResponse> calculate_map_angle_future_;
  QFutureWatcher<CalculateMapAngleResponse> calculate_map_angle_future_watcher_;
  bool is_calculate_map_angle_running_{false};
  mutable std::mutex is_calculate_map_angle_running_mutex_;

  public:
  bool is_initialposition_running() const {
    std::lock_guard guard(is_initialposition_running_mutex_);

    return is_initialposition_running_;
  }

  bool is_secondlocatecalc_running() const {
    std::lock_guard guard(is_secondlocatecalc_running_mutex_);

    return is_secondlocatecalc_running_;
  }

  bool is_movetotarget_thread_running() const {
    std::lock_guard guard(is_movetotarget_thread_running_mutex_);

    return is_movetotarget_thread_running_;
  }

  bool is_calculate_map_angle_running() const {
    std::lock_guard guard(is_calculate_map_angle_running_mutex_);

    return is_calculate_map_angle_running_;
  }
};

class AGVThreadController : public QObject {
  Q_OBJECT
  public:
  explicit AGVThreadController(QObject *parent = nullptr);
  ~AGVThreadController();

  public:
  // void AgvStartMove(double dist, double speed, int type);
  void AgvStartLineMove(double inc_m, double speed_m_s);
  void AgvStartRotateMove(double inc_rad, double speed_rad_s);
  void AgvStopMove();
  void AgvInitStart(double x_m, double y_m, double angle, int timeout_ms);
  void AgvInitStop();
  void AgvBackToZero(BackToZeroParams params);
  void AgvStartBackChange(double x_mm, double y_mm);
  void StopAgvSystem();
  void AgvMoveToTarget(double _x_pos_mm, double _y_pos_mm);
  void AgvStartMoveToTargetThread();
  void SecondLocateCalc(SecondLocateParams param);
  void CleanUpAgvTcp();
  void CalculateMapAngle(CalculateMapAngleParams params);

  bool is_initialposition_running() const {
    return worker_->is_initialposition_running();
  }

  bool is_secondlocatecalc_running() const {
    return worker_->is_secondlocatecalc_running();
  }

  bool is_movetotarget_thread_running() const {
    return worker_->is_movetotarget_thread_running();
  }

  bool is_calculate_map_angle_running() const {
    return worker_->is_calculate_map_angle_running();
  }

  //传递给Worker的信号
  signals:
  void _sig_do_init(void);
  // void _sig_AgvStartMove(double dist, double speed, int type);
  void _sig_AgvStartLineMove(double inc_m, double speed_m_s);
  void _sig_AgvStartRotateMove(double inc_rad, double speed_rad_s);
  void _sig_AgvStopMove();
  void _sig_AgvInitStart(double x_m, double y_m, double angle, int timeout_ms);
  void _sig_AgvInitStop();
  void _sig_AgvBackToZero(BackToZeroParams params);
  void _sig_AgvStartBackChange(double x_mm, double y_mm);
  void _sig_StopAgvSystem();
  void _sig_AgvMoveToTarget(double _x_pos, double _y_pos);
  void _sig_AgvStartMoveToTargetThread();
  void _sig_SecondLocateCalc(SecondLocateParams param);
  void _sig_CleanUpAgvTcp();
  void _sig_CalculateMapAngle(CalculateMapAngleParams params);

  //传递给外界的信号
  signals:
  void sig_CommomMessage(int type, QString msg);

  void sig_SecondLocateMovePlc(secondlocatecalc_result_t result);
  void sig_InitialPositionResult(initialposition_result_t result);
  void sig_CalculateMapAngleResult(CalculateMapAngleResponse result);

  private:
  QThread *thread_{nullptr};
  AGVThreadWorker *worker_{nullptr};
};

#endif // AGVTHREADCONTROLLER_H
